#include "Frustum.h"
#include "MyGL.h"
#include "Vec4.h"

namespace Engine
{
	int Frustum::MakeOutCode(const Vec4& pos) const
	{
		//check if vertex is within frustum
		int code = 0;
	
		if(pos.x < -pos.w) code |= 0x01;
		if(pos.x >  pos.w) code |= 0x02;
		if(pos.y < -pos.w) code |= 0x04;
		if(pos.y >  pos.w) code |= 0x08;
		if(pos.z < -pos.w) code |= 0x10;
		if(pos.z >  pos.w) code |= 0x20;
	
		return code;
	}
	
	void Frustum::ReCalc()
	{
		//multiply matrix by projection and modelview matrices
		Matrix modelView, projection;
		modelView.SetToModelViewMatrix();
		projection.SetToProjectionMatrix();
	
		m_clipMatrix = modelView * projection;

		//work out planes
		m_planes[0].SetABCD(   m_clipMatrix[0] + m_clipMatrix[12],     m_clipMatrix[1] + m_clipMatrix[13],     m_clipMatrix[2] + m_clipMatrix[14],  -(m_clipMatrix[3] + m_clipMatrix[15]));
		m_planes[1].SetABCD( -(m_clipMatrix[0] + m_clipMatrix[12]),  -(m_clipMatrix[1] + m_clipMatrix[13]),  -(m_clipMatrix[2] + m_clipMatrix[14]), -(-m_clipMatrix[3] + m_clipMatrix[15]));

		m_planes[2].SetABCD(   m_clipMatrix[4] + m_clipMatrix[12],     m_clipMatrix[5] + m_clipMatrix[13],     m_clipMatrix[6] + m_clipMatrix[14],  -(m_clipMatrix[7] + m_clipMatrix[15]));
		m_planes[3].SetABCD( -(m_clipMatrix[4] + m_clipMatrix[12]),  -(m_clipMatrix[5] + m_clipMatrix[13]),  -(m_clipMatrix[6] + m_clipMatrix[14]), -(-m_clipMatrix[7] + m_clipMatrix[15]));

		m_planes[4].SetABCD(   m_clipMatrix[8] + m_clipMatrix[12],     m_clipMatrix[9] + m_clipMatrix[13],    m_clipMatrix[10] + m_clipMatrix[14],  -(m_clipMatrix[11] + m_clipMatrix[15]));
		m_planes[5].SetABCD( -(m_clipMatrix[8] + m_clipMatrix[12]),  -(m_clipMatrix[9] + m_clipMatrix[13]), -(m_clipMatrix[10] + m_clipMatrix[14]), -(-m_clipMatrix[11] + m_clipMatrix[15]));
	}

	bool Frustum::Intersects(const Vec3& point) const
	{
		return MakeOutCode(const_cast <Matrix&> (m_clipMatrix) * Vec4(point)) == 0;
	}
	/*
	bool Frustum::Intersects(const AABB& b) const
	{
		//small chance of false positives

		int total = 0x3f;//6 planes, 0000 0000 0000 0000 0000 0000 0011 1111

		for(unsigned int n = 0; n < 8; ++n)
		{
			total &= MakeOutCode(const_cast <Matrix&> (m_clipMatrix) * b.GetCorner(n));

			if(!total) return true;//possibly in frustum
		}

		return false;//not in frustum
	}*/

	bool Frustum::Intersects(const Sphere& s) const
	{
		for(int i = 0; i < 6; ++i)
			if(m_planes[i].GetDistanceToPoint(s.pos) < -s.radius) return true;

		return false;
	}
}